using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using MathNet.Numerics.LinearAlgebra;

public class TryIK : MonoBehaviour
{
    public Transform target;
    public Transform[] bones_Tail2Root;
    public bool isUpdate = true;
    public int iterateLimit = 5;
    public float precision = 0.01f;
    public float step = 0.003f;

    private float end2targetDisSqrt;
    private List<Vector3> jointsRotateAxis;
    private Matrix<float> m_arrJacobian, m_arrJacobianTransposed;
    private Vector<float> effectorVector, DqArr;
    private int _level;

    private void Start()
    {
        _level = bones_Tail2Root.Length - 1;
        jointsRotateAxis = new List<Vector3>(new Vector3[_level]);

        // 初始化Jacobian矩阵
        m_arrJacobian = Matrix<float>.Build.Dense(6, _level);
        m_arrJacobianTransposed = Matrix<float>.Build.Dense(_level, 6);

        // 初始化Vector
        effectorVector = Vector<float>.Build.Dense(6);
        DqArr = Vector<float>.Build.Dense(_level);
    }

    void Update()
    {
        if (isUpdate)
        {
            JTsolve();
        }
    }

    void JTsolve()
    {
        for (int i = 0; i < iterateLimit; ++i)
        {
            Vector3 jointEndPos = bones_Tail2Root[0].position;
            Vector3 end2target = jointEndPos - target.position;

            end2targetDisSqrt = end2target.sqrMagnitude;
            if (end2targetDisSqrt <= precision) break;

            // 使用 `DenseOfArray` 直接创建 Vector<float>
            effectorVector = Vector<float>.Build.DenseOfArray(new float[] { end2target.x, end2target.y, end2target.z, 0, 0, 0 });

            // 计算Jacobian矩阵
            for (int column = 0, jointIndex = 1; column < _level; ++column, ++jointIndex)
            {
                Vector3 jointPos = bones_Tail2Root[jointIndex].position;
                Vector3 joint2Target = target.position - jointPos;
                Vector3 joint2End = jointEndPos - jointPos;

                Vector3 jointAxis = Vector3.Cross(joint2Target, joint2End).normalized;
                jointsRotateAxis[column] = jointAxis;

                Vector3 vecEntry = Vector3.Cross(joint2End, jointAxis);

                // 设置 Jacobian 矩阵的一列
                m_arrJacobian.SetColumn(column, Vector<float>.Build.DenseOfArray(new float[] { vecEntry.x, vecEntry.y, vecEntry.z, 0, 0, 0 }));
            }

            // 计算Jacobian的转置并求出DqArr
            m_arrJacobianTransposed = m_arrJacobian.Transpose();
            DqArr = m_arrJacobianTransposed * effectorVector;

            // 旋转每个关节
            for (int jointIndex = 1; jointIndex <= _level; ++jointIndex)
            {
                Quaternion q = Quaternion.AngleAxis(DqArr[jointIndex - 1] * step, jointsRotateAxis[jointIndex - 1]);
                bones_Tail2Root[jointIndex].rotation = q * bones_Tail2Root[jointIndex].rotation;
            }
        }
    }
}
